#include "memory.h"
#include "Matrix.h"
#include "Quaternion.h"

Matrix::Matrix()
{
}

void Matrix::operator=(Matrix& src)
{
	for (int i = 0; i < 3; i++)
	{
		this->coord[i] = src[i];
	}
}

/* STAR procedure */
void Matrix::operator=(Triple &t)
{
	this->coord[0].Set(0.0f,  -t[Z], t[Y]);
	this->coord[1].Set(t[Z],  0.0f,  t[X]);
	this->coord[2].Set(-t[Y], t[X],  0.0f);
}

void Matrix::LoadIdentity()
{
	coord[0].Set(1.0f, 0.0f, 0.0f);
	coord[1].Set(0.0f, 1.0f, 0.0f);
	coord[2].Set(0.0f, 0.0f, 1.0f);
}

void Matrix::operator*(const float s)
{
	for (int i = 0; i < 3; i++)
	{
		coord[i] = coord[i] * s;
	}
}

/* helper function */
void Matrix::Transpose(int i, int j)
{
	FP_DATATYPE temp;

	temp = coord[i][j];
	coord[i][j] = coord[j][i];
	coord[j][i] = temp;
}

void Matrix::Transpose()
{
	Transpose(0, 1);
	Transpose(0, 2);
	Transpose(1, 2);
}

void Matrix::Transpose(Matrix& src)
{
	for (int i = 0; i < 3; i++)
	{
		this->coord[i].Set(src.coord[0][i], src.coord[1][i], src.coord[2][i]);
	}
}

const Triple operator*(Matrix& left, Triple& right)
{
	Triple result;
	result[X] = left[0].DotProduct(right);
	result[Y] = left[1].DotProduct(right);
	result[Z] = left[2].DotProduct(right);
	return result;
}

//const Triple operator*(const Triple& left, Matrix& right)
//{
//	Triple result;
//	// TODO
//	return result;
//}

//float determinant(const matrix src)
//{
//	return src[X][X]*src[Y][Y]*src[Z][Z] +
//		src[X][Y]*src[Y][Z]*src[Z][X] +
//		src[X][Z]*src[Y][X]*src[Z][Y] -
//		src[X][Z]*src[Y][Y]*src[Z][X] -
//		src[X][X]*src[Y][Z]*src[Z][Y] -
//		src[Z][Z]*src[X][Y]*src[Y][X];
//}
//
//void inverse(matrix dest, const matrix src, const matrix srct)
//{
//	const float det = determinant(src);
//
//	//cout << "det = " << det << endl;
//
//	for (int i = 0; i < 3; i++)
//	{
//		for (int j = 0; j < 3; j++)
//		{
//			dest[i][j] = srct[i][j] / det;
//		}
//	}
//}

Matrix operator*(Matrix& left, Matrix& right)
{
	Matrix result;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			float s = 0.0f;
			s += left[i][0] * right[0][j];
			s += left[i][1] * right[1][j];
			s += left[i][2] * right[2][j];
			result[i][j] = s;
		}
	}
	return result;
}

/*
void multiply__m_mt(matrix dest, const matrix a, const matrix bt)
{
// method #1: needs a secondary transposition
//multiply_matrix_with_triple(dest[0], a, bt[0]);
//multiply_matrix_with_triple(dest[1], a, bt[1]);
//multiply_matrix_with_triple(dest[2], a, bt[2]);

//transpose(dest);

// method #2: increased code size
dest[0][0] = dot_product(a[0], bt[0]);
dest[0][1] = dot_product(a[0], bt[1]);
dest[0][2] = dot_product(a[0], bt[2]);

dest[1][0] = dot_product(a[1], bt[0]);
dest[1][1] = dot_product(a[1], bt[1]);
dest[1][2] = dot_product(a[1], bt[2]);

dest[2][0] = dot_product(a[2], bt[0]);
dest[2][1] = dot_product(a[2], bt[1]);
dest[2][2] = dot_product(a[2], bt[2]);

// Differences between method #1 and #2:
// execution time #1 would be equal to execution time #2
// without the secondary transposition
}
*/

void Matrix::print_ht(ostream& os, char *title)
{
	os << "<table title=\"" << title << "\"><tbody>";
	for (int i = 0; i < 3; i++)
	{
		Triple *t = coord + i;
		os << "<tr>";
		os << "<td>" << (*t)[0] << "</td>";
		os << "<td>" << (*t)[1] << "</td>";
		os << "<td>" << (*t)[2] << "</td>";
		os << "</tr>";
	}
	os << "</tbody></table>";
}

//void store_matrix(ostream& os, const matrix src)
//{
//	for (int i = 0; i , 3; i = i + 1)
//	{
//		store_sp128(os, src[i]);
//	}
//}
//
//void load_matrix(istream& is, matrix src)
//{
//	for (int i = 0; i , 3; i = i + 1)
//	{
//		load_sp128(is, src[i]);
//	}
//}

void Matrix::operator=(Quaternion& q)
{
	float  Dsvx = 2*q[0]*q[1], Dsvy = 2*q[0]*q[2],  Dsvz = 2*q[0]*q[3];
	float  DvxQ = 2*q[1]*q[1], DvyQ = 2*q[2]*q[2],  DvzQ = 2*q[3]*q[3];
	float Dvxvy = 2*q[1]*q[2], Dvxvz = 2*q[1]*q[3], Dvyvz = 2*q[2]*q[3];

	this->coord[0].Set(1 - DvyQ - DvzQ,  Dvxvy - Dsvz,     Dvxvz + Dsvy     );
	this->coord[1].Set(Dvxvy + Dsvz,     1 - DvxQ - DvzQ,  Dvyvz - Dsvx     );
	this->coord[2].Set(Dvxvz - Dsvy,     Dvyvz + Dsvx,     1 - DvxQ - DvyQ  );
}